#!/usr/bin/env python3

import math
import os
import rclpy
from rclpy.node import Node
from geometry_msgs.msg import PoseWithCovarianceStamped
from cbs_srv_interfaces.msg import TPoint # type: ignore
from cbs_srv_interfaces.msg import Traj # type: ignore
import time
import yaml

veh_id = "13"
target_string = "write_end"
pose_file_path = '/home/nvidia/catkin_ws_brige_data/src/robot_data_brige/pose_file.yaml'  # 在叉车端请指定文件绝对路径
traj_file_path = '/home/nvidia/catkin_ws_brige_data/src/robot_data_brige/traj_file.yaml'

class RobotPoseForwarder(Node):
    def __init__(self):
        self.x = 0.0
        self.y = 0.0
        self.yaw = 0.0
        self.isGetPose = False
        super().__init__('robot_data_brige')
        
        while True:
            self.get_logger().info("Start read pose file...")
            with open(pose_file_path, 'r') as f:
                pose_node = f.read()
            if target_string in pose_node:
                self.get_logger().info("The vehicle position was successfully read")
                try:
                    content = yaml.safe_load(pose_node)
                    self.x = content['x']
                    self.y = content['y']
                    self.yaw = content['yaw']
                    self.isGetPose = True
                    with open(pose_file_path, 'w') as cf:
                        cf.truncate(0)  # 清空文件
                        self.get_logger().info("The pose_file.yaml has been cleared successfully")
                except yaml.YAMLError as e:
                    self.get_logger().info(f"Error parsing YAML: {e}")
                break
            else:
                self.get_logger().info(f"'{target_string}' not found, waiting and retrying...")
            time.sleep(1)

        pub_pose_topic_name = "pcl_pose_" + veh_id
        self.pose_publisher = self.create_publisher(
            PoseWithCovarianceStamped,
            pub_pose_topic_name,
            10  # Queue size
        )
        
        if not self.isGetPose:
            self.get_logger().info("File reading pose_file.yaml")
            return
        p = PoseWithCovarianceStamped()
        p.header.stamp = self.get_clock().now().to_msg()
        p.header.frame_id = "map"
        p.pose.pose.position.x = self.x
        p.pose.pose.position.y = self.y
        p.pose.pose.position.z = 0.0
        p.pose.pose.orientation.x = 0.0
        p.pose.pose.orientation.y = 0.0
        p.pose.pose.orientation.z = math.sin(self.yaw/2)
        p.pose.pose.orientation.w = math.cos(self.yaw/2)
 
        i = 10
        while i > 0:
            self.pose_publisher.publish(p)
            self.get_logger().info(f'Publishing Pose message: [{self.x}, {self.y}, {self.yaw}]')
            i -= 1
            time.sleep(0.5)
    
        
        # 轨迹
        sub_traj_topic_name = "opt_traj_" + veh_id
        self.traj_subscription = self.create_subscription(
            Traj,
            sub_traj_topic_name,
            self.traj_callback,
            10
        )
        self.traj_subscription
        self.get_logger().info("Pose and Traj Forwarder Node Started")
        
        self.isWrittenTraj = False    
    def traj_callback(self, msg):
        self.get_logger().info('Receive opt_traj message...')
        if not self.isWrittenTraj:
            # 写入文件
            data = []
            for tp in msg.opt_traj:
                data.append({'x': round(float(tp.x), 4),
                             'y': round(float(tp.y), 4), 
                             'v': round(float(tp.v), 4), 
                             'theta': round(float(tp.x), 4),
                             'phi': round(float(tp.phi), 4),
                             'acc': round(float(tp.acc), 4),
                             'dphi': round(float(tp.dphi), 4)
                             })
            traj = {'agent': data, 'write_end': 1}
            
            class MyDumper(yaml.Dumper):
                def increase_indent(self, flow=False, indentless=False):
                    return super(MyDumper, self).increase_indent(flow=flow, indentless=False)
            with open(traj_file_path, 'w') as file:
                yaml.dump(traj, file, default_flow_style=False, indent=2, Dumper=MyDumper)
            self.get_logger().info('The trajectory has been written to the file') 
            rclpy.shutdown()    
        

def main(args=None):
    rclpy.init(args=args)
    forwarder_node = RobotPoseForwarder()
    rclpy.spin(forwarder_node)

    # Cleanup
    rclpy.shutdown()

if __name__ == '__main__':
    main()

